import time
from robomaster import robot
from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster
import pos_nokov as pos
import sympy as sp
import math
import utils
import random
import msvcrt
from interval import Interval


done=False
field_error=False
# field_range=[[-1000,2700],[-1300,1700]]
x_range=Interval(-1150, 3000)
y_range=Interval(-1400,1700)
power=0
posi_target=[40,40]
z=0
global poses, pos_ball

def led_red_solid(robot_group):
    """红灯常亮"""
    robot_group.led.set_led(led.COMP_ALL, 255, 1, 1, led.EFFECT_ON)

def led_grey_solid(robot_group):
    """grey常亮"""
    robot_group.led.set_led(led.COMP_ALL, 75, 255, 255, led.EFFECT_ON)

def get_pass_posi(pos_ball, pos_mate, ready=True):
    bx=pos_ball[0]
    by=pos_ball[1]
    mx=pos_mate[0]
    my=pos_mate[1]
    x,y=sp.symbols('x, y')
    distance=400 if ready else 310
    solutions=sp.nonlinsolve([(x-bx)*(by-my)-(y-by)*(bx-mx),
                              (x-bx)**2+(y-by)**2-distance**2],[x,y])
    # print(solutions)
    for s in solutions:
        if not ((bx<s[0]<mx or bx>s[0]>mx) and (by<s[1]<my or by>s[1]>my)):
            posi=s
    return posi

def get_pass_ori(pos_ball, pos_mate):
    bx=pos_ball[0]
    by=pos_ball[1]
    mx=pos_mate[0]
    my=pos_mate[1]
    ori=math.atan2(my-by, mx-bx)*180/math.pi
    return ori

def calc_ready_catch_posi():
    global power,posi_target,z,field_error
    posi=get_pass_posi(pos_ball, poses[1-power][1])
    # print('目标位置:%.3f, y:%.3f' %(posi[0],posi[1]))
    if posi[0] not in x_range or posi[1] not in y_range:
        print('超出正常范围，target全局坐标为x:%.3f, y:%.3f' %(posi[0],posi[1]))
        field_error=True
    # print(posi)
    posi_target=utils.trans_relative_co(poses[power], posi)
    ori=get_pass_ori(pos_ball, poses[1-power][1])
    z=ori-poses[power][0]
    if z>180:
        z=z-360
    elif z<-180:
        z=z+360
        
def calc_catch_pose():
    global power,posi_target,z,field_error
    posi=get_pass_posi(pos_ball, poses[1-power][1], False)
    if posi[0] not in x_range or posi[1] not in y_range:
        print('超出正常范围，target全局坐标为x:%.3f, y:%.3f' %(posi[0],posi[1]))
        field_error=True
    # print(posi)
    posi_target=utils.trans_relative_co(poses[power], posi)
    ori=get_pass_ori(pos_ball, poses[1-power][1])
    z=ori-poses[power][0]
    if z>180:
        z=z-360
    elif z<-180:
        z=z+360

def init_arm(robot_group):
    t1=time.time()
    ep_arm = robot_group.robotic_arm
    ep_arm.move(x=100, y=-100).wait_for_completed()
    ep_arm.move(x=-90, y=0).wait_for_completed()
    t2=time.time()
    print(t2-t1)

def move_to_follow(robot_group):
    global posi_target,z
    # print('##### rotate: %.3f' % z)
    # print('x: % .3f, y: % .3f, z: % .1f' % (posi_target[0]/1000,-posi_target[1]/1000,z))
    if posi_target[0] in Interval(-30,30) and -30<posi_target[1]<30:
        print('--acurate')
        robot_group.chassis.drive_speed(x=posi_target[0]/1000, y=-posi_target[1]/500, z=-z, timeout=1)
    else:
        robot_group.chassis.drive_speed(posi_target[0]/1000,-posi_target[1]/1000,-z/1,timeout=2) #z的方向，speed和move相反
    time.sleep(0.2)

def stop(robot_group):
    robot_group.chassis.drive_wheels(w1=0, w2=0, w3=0, w4=0,timeout=1)
    time.sleep(0.01)
    
# def catch_simple(robot_group):
#     ep_arm = robot_group.robotic_arm
#     ep_chassis = robot_group.chassis
#     ep_gripper=robot_group.gripper
    
#     # ep_arm.move(y=-20).wait_for_completed()
#     ep_gripper.open(power=100)
#     time.sleep(1)
#     ep_gripper.pause()
#     ep_chassis.drive_speed(0.1, 0, 0, timeout=1)
#     time.sleep(0.95)
#     ep_chassis.drive_speed(0, 0, 0, timeout=1)
#     time.sleep(0.01)
#     ep_gripper.close(power=50)
#     time.sleep(1)
#     ep_gripper.pause()
#     ep_arm.move(x=0, y=20).wait_for_completed()

def catch(robot_group):
    global poses, done
    robot_group.gripper.open(power=100)
    time.sleep(1)
    robot_group.gripper.pause()
    while(True):
        poses=[pos.get_device_pose(1),pos.get_device_pose(2)]
        calc_catch_pose()
        print('catching, x:%.3f, y:%.3f, z:%.2f' % (posi_target[0],posi_target[1],z))
        robot_group.chassis.drive_speed(posi_target[0]/1000,-posi_target[1]/1000,-z/1,timeout=2)
        if -12<posi_target[0]<12 and -11<posi_target[1]<11 and -3<z<3:
            print('catched')
            robot_group.gripper.close(power=100)
            time.sleep(1)
            robot_group.gripper.pause()
            robot_group.robotic_arm.move(x=0, y=20).wait_for_completed()
            break
        if (msvcrt.kbhit() and msvcrt.getch() == b'p') or field_error:
            robot_group.chassis.drive_speed(x=0, y=0, z=0, timeout=2)
            time.sleep(0.1)
            print("Paused...")
            # Wait until the 'p' key is repressed
            while msvcrt.getch() != b'p':
                time.sleep(0.5)
                msvcrt.getch()
            print("Resumed")
        if msvcrt.kbhit() and msvcrt.getch() == b't':
            done=True
            break
                      
    
def move_and_shoot(robot_group):
    # robot_group.chassis.move(0,0.4 if poses[power][1][1]*math.cos(poses[power][0]*math.pi/180)>0 
                            # else -0.4,0,1.5).wait_for_completed()
    t1=time.time()
    ep_arm = robot_group.robotic_arm
    ep_chassis = robot_group.chassis
    ep_gripper=robot_group.gripper
    
    t2=time.time()
    # ep_arm.move(x=1, y=-20).wait_for_completed()
    # time.sleep(0.8)
    t3=time.time()
    ep_gripper.open(power=50)
    time.sleep(0.5)
    ep_gripper.pause()
    t4=time.time()
    ep_chassis.drive_speed(-1,0,0)
    time.sleep(0.1)
    ep_chassis.drive_speed(0,0,0)
    time.sleep(0.01)
    ep_arm.move(y=80).wait_for_completed()
    # ep_chassis.move(x=-0.3,xy_speed=3).wait_for_completed()
    ep_chassis.drive_speed(1.5,0,0)
    time.sleep(0.4)
    ep_chassis.drive_speed(0,0,0)
    time.sleep(0.01)
    ep_chassis.move(x=-0.5,xy_speed=3).wait_for_completed()
    ep_arm.move(x=0, y=-100).wait_for_completed()
    print('time consumed: %.3f, %.3f, %.3f' %(t2-t1, t3-t2, t4-t3))

def run():
    global poses, pos_ball, power, done,posi_target,z,field_error
    # robomaster.config.LOCAL_IP_STR = "192.168.1.111"
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', 
                      '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()

    number = multi_robots.number_id_by_sn([2, robots_sn_list[1]], [3, robots_sn_list[2]])
    print("The number of robot is: {0}".format(number))
    g_all = multi_robots.build_group([2, 3])
    g_red = multi_robots.build_group([2])
    g_grey = multi_robots.build_group([3])
    groups=[g_red, g_grey]
    multi_robots.run([g_grey, led_grey_solid])
    multi_robots.run([g_red, led_red_solid])
    multi_robots.run([g_all, init_arm])
    print(power)
    while(not done):
        while(True):
            poses=[pos.get_device_pose(1),pos.get_device_pose(2)]
            # print(poses)
            print('readying: x:%.3f, y:%.3f, z:%.2f' % (posi_target[0],posi_target[1],z))
            pos_ball=pos.get_ball_pose()
            field_error=False
            if pos_ball[0] not in x_range or pos_ball[1] not in y_range:
                print('球超出正常范围，当前目标坐标为x:%.3f, y:%.3f' %(pos_ball[0],pos_ball[1]))
                field_error=True
            calc_ready_catch_posi()
            
            if -12<posi_target[0]<12 and -12<posi_target[1]<12 and -3<z<3:
                print('###############completed series acitons')
                multi_robots.run([groups[power],stop])
                multi_robots.run([groups[power],catch])
                multi_robots.run([groups[power],move_and_shoot])
                power=1-power
                break
            else:
                # pass
                multi_robots.run([groups[power],move_to_follow])
                    
            if (msvcrt.kbhit() and msvcrt.getch() == b'p') or field_error:
                multi_robots.run([groups[power],stop])
                print("Paused...")
                # Wait until the 'p' key is repressed
                while msvcrt.getch() != b'p':
                    time.sleep(0.5)
                    msvcrt.getch()
                print("Resumed")
            if msvcrt.kbhit() and msvcrt.getch() == b't':
                done=True
                break
                        
    multi_robots.close()